package twoD.IsoPfem;

import inf.jlinalg.Array2DMatrix;
import inf.jlinalg.ArrayVector;
import inf.jlinalg.BLAM;
import inf.jlinalg.ELAM;
import inf.jlinalg.IMatrix;
import inf.jlinalg.IVector;
import inf.jlinalg.Vector2D;
import twoD.IsoPfem.Structure.CType;
import twoD.IsoPfem.math.FMath;
import twoD.IsoPfem.math.FunctionRToR;
import twoD.IsoPfem.math.GaussPointsT;
import twoD.IsoPfem.math.Integrator;
import twoD.IsoPfem.math.Interval;
import twoD.IsoPfem.math.ShapeFunctionOnQT;

/**
 * implementation class for quadratic triangle
 * 
 * @author hbui
 * 
 */
public class CST2 extends Element2D {

	private ShapeFunctionOnQT N_;

	public CST2(IGeometry P) {
		super(P);
		setCType(CType.PLANE_STRESS);
		N_ = new ShapeFunctionOnQT();
	}

	public CST2(RealTable realTable, IGeometry P) {
		this(P);
		setRealTable(realTable);
	}

	@Override
	public double getVolume() {
		Vector2D p1 = getNodes(0).getPosition();
		Vector2D p2 = getNodes(1).getPosition();
		Vector2D p3 = getNodes(2).getPosition();
		Vector2D d13 = p1.subtract(p3);
		Vector2D d23 = p2.subtract(p3);
		double dx1n13 = d13.getX1();
		double dx1n23 = d23.getX1();
		double dx2n13 = d13.getX2();
		double dx2n23 = d23.getX2();
		double detJ = dx1n13 * dx2n23 - dx1n23 * dx2n13;

		return getThickness() * Math.abs(detJ) / 2;
	}

	private IMatrix computeJacobian(double[] X_, double[] Y_, double xi1,
			double xi2, double xi3) {
		// compute Jacobian matrix
		IMatrix J = new Array2DMatrix(3, 3);
		double[] dN1dxi = N_.getBasisFunction(0).gradientAt(xi1, xi2, xi3);
		double[] dN2dxi = N_.getBasisFunction(1).gradientAt(xi1, xi2, xi3);
		double[] dN3dxi = N_.getBasisFunction(2).gradientAt(xi1, xi2, xi3);
		double[] dN4dxi = N_.getBasisFunction(3).gradientAt(xi1, xi2, xi3);
		double[] dN5dxi = N_.getBasisFunction(4).gradientAt(xi1, xi2, xi3);
		double[] dN6dxi = N_.getBasisFunction(5).gradientAt(xi1, xi2, xi3);
		double Jx1 = X_[0] * dN1dxi[0] + X_[1] * dN2dxi[0] + X_[2] * dN3dxi[0]
				+ X_[3] * dN4dxi[0] + X_[4] * dN5dxi[0] + X_[5] * dN6dxi[0];
		double Jx2 = X_[0] * dN1dxi[1] + X_[1] * dN2dxi[1] + X_[2] * dN3dxi[1]
				+ X_[3] * dN4dxi[1] + X_[4] * dN5dxi[1] + X_[5] * dN6dxi[1];
		double Jx3 = X_[0] * dN1dxi[2] + X_[1] * dN2dxi[2] + X_[2] * dN3dxi[2]
				+ X_[3] * dN4dxi[2] + X_[4] * dN5dxi[2] + X_[5] * dN6dxi[2];
		double Jy1 = Y_[0] * dN1dxi[0] + Y_[1] * dN2dxi[0] + Y_[2] * dN3dxi[0]
				+ Y_[3] * dN4dxi[0] + Y_[4] * dN5dxi[0] + Y_[5] * dN6dxi[0];
		double Jy2 = Y_[0] * dN1dxi[1] + Y_[1] * dN2dxi[1] + Y_[2] * dN3dxi[1]
				+ Y_[3] * dN4dxi[1] + Y_[4] * dN5dxi[1] + Y_[5] * dN6dxi[1];
		double Jy3 = Y_[0] * dN1dxi[2] + Y_[1] * dN2dxi[2] + Y_[2] * dN3dxi[2]
				+ Y_[3] * dN4dxi[2] + Y_[4] * dN5dxi[2] + Y_[5] * dN6dxi[2];
		J.set(0, 0, 1);
		J.set(0, 1, 1);
		J.set(0, 2, 1);
		J.set(1, 0, Jx1);
		J.set(1, 1, Jx2);
		J.set(1, 2, Jx3);
		J.set(2, 0, Jy1);
		J.set(2, 1, Jy2);
		J.set(2, 2, Jy3);
		return J;
	}

	private double computeDetJ(double[] X_, double[] Y_, double xi1,
			double xi2, double xi3) {
		IMatrix J = computeJacobian(X_, Y_, xi1, xi2, xi3);
		double Jx13 = J.get(1, 0) - J.get(1, 2);
		double Jx21 = J.get(1, 1) - J.get(1, 0);
		// double Jx32 = J.get(1, 2) - J.get(1, 1);
		double Jy31 = J.get(2, 2) - J.get(2, 0);
		double Jy12 = J.get(2, 0) - J.get(2, 1);
		// double Jy23 = J.get(2, 1) - J.get(2, 2);
		return Jx21 * Jy31 - Jy12 * Jx13;
	}

	private double computeB(IMatrix B, double xi1, double xi2, double xi3) {
		double[] X1 = getNodes(0).getPosition().toArray();
		double[] X2 = getNodes(1).getPosition().toArray();
		double[] X3 = getNodes(2).getPosition().toArray();
		double[] X4 = getEdges(0).getPoint(0.5);
		double[] X5 = getEdges(1).getPoint(0.5);
		double[] X6 = getEdges(2).getPoint(0.5);
		double[] X_ = new double[] { X1[0], X2[0], X3[0], X4[0], X5[0], X6[0] };
		double[] Y_ = new double[] { X1[1], X2[1], X3[1], X4[1], X5[1], X6[1] };

		double[] dN1dxi = N_.getBasisFunction(0).gradientAt(xi1, xi2, xi3);
		double[] dN2dxi = N_.getBasisFunction(1).gradientAt(xi1, xi2, xi3);
		double[] dN3dxi = N_.getBasisFunction(2).gradientAt(xi1, xi2, xi3);
		double[] dN4dxi = N_.getBasisFunction(3).gradientAt(xi1, xi2, xi3);
		double[] dN5dxi = N_.getBasisFunction(4).gradientAt(xi1, xi2, xi3);
		double[] dN6dxi = N_.getBasisFunction(5).gradientAt(xi1, xi2, xi3);

		IMatrix J = computeJacobian(X_, Y_, xi1, xi2, xi3);

		double Jx13 = J.get(1, 0) - J.get(1, 2);
		double Jx21 = J.get(1, 1) - J.get(1, 0);
		double Jx32 = J.get(1, 2) - J.get(1, 1);
		double Jy31 = J.get(2, 2) - J.get(2, 0);
		double Jy12 = J.get(2, 0) - J.get(2, 1);
		double Jy23 = J.get(2, 1) - J.get(2, 2);
		double detJ = Jx21 * Jy31 - Jy12 * Jx13;

		// compute dNdX
		double dN1dx = (Jy23 * dN1dxi[0] + Jy31 * dN1dxi[1] + Jy12 * dN1dxi[2])
				/ detJ;
		double dN2dx = (Jy23 * dN2dxi[0] + Jy31 * dN2dxi[1] + Jy12 * dN2dxi[2])
				/ detJ;
		double dN3dx = (Jy23 * dN3dxi[0] + Jy31 * dN3dxi[1] + Jy12 * dN3dxi[2])
				/ detJ;
		double dN4dx = (Jy23 * dN4dxi[0] + Jy31 * dN4dxi[1] + Jy12 * dN4dxi[2])
				/ detJ;
		double dN5dx = (Jy23 * dN5dxi[0] + Jy31 * dN5dxi[1] + Jy12 * dN5dxi[2])
				/ detJ;
		double dN6dx = (Jy23 * dN6dxi[0] + Jy31 * dN6dxi[1] + Jy12 * dN6dxi[2])
				/ detJ;

		double dN1dy = (Jx32 * dN1dxi[0] + Jx13 * dN1dxi[1] + Jx21 * dN1dxi[2])
				/ detJ;
		double dN2dy = (Jx32 * dN2dxi[0] + Jx13 * dN2dxi[1] + Jx21 * dN2dxi[2])
				/ detJ;
		double dN3dy = (Jx32 * dN3dxi[0] + Jx13 * dN3dxi[1] + Jx21 * dN3dxi[2])
				/ detJ;
		double dN4dy = (Jx32 * dN4dxi[0] + Jx13 * dN4dxi[1] + Jx21 * dN4dxi[2])
				/ detJ;
		double dN5dy = (Jx32 * dN5dxi[0] + Jx13 * dN5dxi[1] + Jx21 * dN5dxi[2])
				/ detJ;
		double dN6dy = (Jx32 * dN6dxi[0] + Jx13 * dN6dxi[1] + Jx21 * dN6dxi[2])
				/ detJ;

		B.set(0, 0, dN1dx);
		B.set(1, 1, dN1dy);
		B.set(2, 0, dN1dy);
		B.set(2, 1, dN1dx);

		B.set(0, 2, dN2dx);
		B.set(1, 3, dN2dy);
		B.set(2, 2, dN2dy);
		B.set(2, 3, dN2dx);

		B.set(0, 4, dN3dx);
		B.set(1, 5, dN3dy);
		B.set(2, 4, dN3dy);
		B.set(2, 5, dN3dx);

		B.set(0, 6, dN4dx);
		B.set(1, 7, dN4dy);
		B.set(2, 6, dN4dy);
		B.set(2, 7, dN4dx);

		B.set(0, 8, dN5dx);
		B.set(1, 9, dN5dy);
		B.set(2, 8, dN5dy);
		B.set(2, 9, dN5dx);

		B.set(0, 10, dN6dx);
		B.set(1, 11, dN6dy);
		B.set(2, 10, dN6dy);
		B.set(2, 11, dN6dx);

		return detJ;
	}

	private IMatrix computeBtCBdetJ(double... xi) {
		IMatrix B = new Array2DMatrix(3, 12);
		double detJ = computeB(B, xi[0], xi[1], xi[2]);
		// System.out.println("B = " + MatrixFormat.format(B));
		IMatrix k = new Array2DMatrix(12, 12);
		ELAM.btCB(B, getCMatrix(), detJ * getThickness() / 2, k);
		return k;
	}

	@Override
	public IVector computeInternalForceVector() {
		// it is the linear element so we can use the method below but the
		// precise way to compute internal force vector is to integrate over the
		// element
		IMatrix ke = computeTangentStiffnessMatrix();
		IVector ue = getUe();
		IVector rie = new ArrayVector(ue.getSize());
		BLAM.multiply(1.0, BLAM.NO_TRANSPOSE, ke, ue, 0.0, rie);
		// IVector rie = new ArrayVector(6);
		// IMatrix B = getBMatrix();
		// IVector sigma = computeSigma();
		// BLAM.multiply(getVolume(), BLAM.TRANSPOSE, B, sigma, 0.0, rie);
		return rie;
	}

	@Override
	public IMatrix computeTangentStiffnessMatrix() {
		IMatrix ke = new Array2DMatrix(12, 12);
		int p = 2;
		int n = GaussPointsT.getNumOfIntegrationPoints(p);
		for (int i = 0; i < n; i++) {
			double[] xi = GaussPointsT.getPoint(p, i);
			final double wi = GaussPointsT.getWeight(p, i);
			// System.out.println("xi = " + ArrayFormat.format(xi) + ", wi = "
			// + wi);
			BLAM.add(wi, computeBtCBdetJ(xi), ke);
		}
		return ke;
	}

	@Override
	public void computeExternalForceVector() {

		double[] X1 = getNodes(0).getPosition().toArray();
		double[] X2 = getNodes(1).getPosition().toArray();
		double[] X3 = getNodes(2).getPosition().toArray();
		double[] X4 = getEdges(0).getPoint(0.5);
		double[] X5 = getEdges(1).getPoint(0.5);
		double[] X6 = getEdges(2).getPoint(0.5);
		final double[] X_ = new double[] { X1[0], X2[0], X3[0], X4[0], X5[0],
				X6[0] };
		final double[] Y_ = new double[] { X1[1], X2[1], X3[1], X4[1], X5[1],
				X6[1] };

		for (EdgeLoad load : edgeLoad_) {
			Node n1 = load.getN1();
			Node n2 = load.getN2();

			int tmp = 0;
			if (n1 == getNodes(0))
				if (n2 == getNodes(1)) {
					tmp = 1; // n1:0 n2:1
				} else
					tmp = 2; // n1:0 n2:2
			else if (n1 == getNodes(1)) {
				if (n2 == getNodes(0)) {
					tmp = 3; // n1:1 n2:0
				} else
					tmp = 4; // n1:1 n2:2
			} else {
				if (n2 == getNodes(0)) {
					tmp = 5; // n1:2 n2:0
				} else
					tmp = 6; // n1:2 n2:1
			}

			final int case_ = tmp;
			FunctionRToR N1 = new FunctionRToR() {

				@Override
				public double valueAt(double x) {
					if (case_ == 1)// n1:0 n2:1
						return x * (2 * x - 1)
								* computeDetJ(X_, Y_, 1 - x, x, 0);
					else if (case_ == 2)// n1:0 n2:2
						return x * (2 * x - 1)
								* computeDetJ(X_, Y_, 1 - x, 0, x);
					else if (case_ == 3)// n1:1 n2:0
						return x * (2 * x - 1)
								* computeDetJ(X_, Y_, x, 1 - x, 0);
					else if (case_ == 4)// n1:1 n2:2
						return x * (2 * x - 1)
								* computeDetJ(X_, Y_, 0, 1 - x, x);
					else if (case_ == 5)// n1:2 n2:0
						return x * (2 * x - 1)
								* computeDetJ(X_, Y_, x, 0, 1 - x);
					else
						// n1:2 n2:1
						return x * (2 * x - 1)
								* computeDetJ(X_, Y_, 0, x, 1 - x);
				}

				@Override
				public int getP() {
					return -1; // nonpolynomial function
				}

				@Override
				public Interval getDomain() {
					return new Interval(0, 1);
				}

				@Override
				public double[] getBreakpoints() {
					return null;
				}

				@Override
				public double derivativeAt(double x) {
					return 0;// does not need in this case since we use
								// numerical
								// integration
				}
			};
			FunctionRToR N2 = new FunctionRToR() {

				@Override
				public double valueAt(double x) {
					if (case_ == 1)// n1:0 n2:1
						return (1 - x) * (1 - 2 * x)
								* computeDetJ(X_, Y_, 1 - x, x, 0);
					else if (case_ == 2)// n1:0 n2:2
						return (1 - x) * (1 - 2 * x)
								* computeDetJ(X_, Y_, 1 - x, 0, x);
					else if (case_ == 3)// n1:1 n2:0
						return (1 - x) * (1 - 2 * x)
								* computeDetJ(X_, Y_, x, 1 - x, 0);
					else if (case_ == 4)// n1:1 n2:2
						return (1 - x) * (1 - 2 * x)
								* computeDetJ(X_, Y_, 0, 1 - x, x);
					else if (case_ == 5)// n1:2 n2:0
						return (1 - x) * (1 - 2 * x)
								* computeDetJ(X_, Y_, x, 0, 1 - x);
					else
						// n1:2 n2:1
						return (1 - x) * (1 - 2 * x)
								* computeDetJ(X_, Y_, 0, x, 1 - x);
				}

				@Override
				public int getP() {
					return -1;
				}

				@Override
				public Interval getDomain() {
					return new Interval(0, 1);
				}

				@Override
				public double[] getBreakpoints() {
					return null;
				}

				@Override
				public double derivativeAt(double x) {
					return 0;
				}
			};
			FunctionRToR N3 = new FunctionRToR() {

				@Override
				public double valueAt(double x) {
					if (case_ == 1)// n1:0 n2:1
						return 4 * x * (1 - x)
								* computeDetJ(X_, Y_, 1 - x, x, 0);
					else if (case_ == 2)// n1:0 n2:2
						return 4 * x * (1 - x)
								* computeDetJ(X_, Y_, 1 - x, 0, x);
					else if (case_ == 3)// n1:1 n2:0
						return 4 * x * (1 - x)
								* computeDetJ(X_, Y_, x, 1 - x, 0);
					else if (case_ == 4)// n1:1 n2:2
						return 4 * x * (1 - x)
								* computeDetJ(X_, Y_, 0, 1 - x, x);
					else if (case_ == 5)// n1:2 n2:0
						return 4 * x * (1 - x)
								* computeDetJ(X_, Y_, x, 0, 1 - x);
					else
						// n1:2 n2:1
						return 4 * x * (1 - x)
								* computeDetJ(X_, Y_, 0, x, 1 - x);
				}

				@Override
				public int getP() {
					return -1;
				}

				@Override
				public Interval getDomain() {
					return new Interval(0, 1);
				}

				@Override
				public double[] getBreakpoints() {
					return null;
				}

				@Override
				public double derivativeAt(double x) {
					return 0;
				}
			};

			double f1 = Integrator
					.integrate(FMath.multiply(N1, load.getLoad()));
			double f2 = Integrator
					.integrate(FMath.multiply(N2, load.getLoad()));
			double f3 = Integrator
					.integrate(FMath.multiply(N3, load.getLoad()));
			Vector2D dn = n2.getPosition().subtract(n1.getPosition());
			double l = dn.norm2();
			double c = dn.getX1() / l;
			double s = dn.getX2() / l;
			/*
			 * update nodal force in n1 and n2
			 */
			n1.addNodalForce(-f1 * s, f1 * c);
			n2.addNodalForce(-f2 * s, f2 * c);
			load.getEdge().addNodalForce(new double[] { -f3 * s, f3 * c });
		}
	}

	@Override
	public IVector computeEpsilon(double... xi) {
		IMatrix B = new Array2DMatrix(3, 12);
		computeB(B, xi[0], xi[1], xi[2]);
		IVector Ue = this.getUe();
		// System.out
		// .println("Ue" + this.getId() + " =" + MatrixFormat.format(Ue));
		IVector e = new ArrayVector(3);
		BLAM.multiply(1.0, BLAM.NO_TRANSPOSE, B, Ue, 0.0, e);
		return e;
	}

}
